import queue
from queue import PriorityQueue
import map2d
#描述算法中的节点数据 
class Point:
    """docstring for point"""
    def __init__(self, x = 0, y = 0):
        self.x = x
        self.y = y
def graph_neighbors(current_pos,map):
    # print(current_pos)
    p1 = [current_pos[0]+1,current_pos[1]+0]
    p2 = [current_pos[0]+0,current_pos[1]-1]
    p3 = [current_pos[0]-1,current_pos[1]+0]
    p4 = [current_pos[0]+0,current_pos[1]+1]
    neighbors_point = [p1,p2,p3,p4]
    real_neighbors_point = list()
    for j in neighbors_point:
        if j[0] > 0 and j[0] < 19 and j[1] > 0 and j[1] < 9 and map.isPass(Point(j[1],j[0])) == True:
            real_neighbors_point.append([j])
            print(j)
    return real_neighbors_point

if __name__ == '__main__':
    ##构建地图
    mapTest = map2d.map2d();
    mapTest.showMap();
    start = [[1,1]]
    goal = [18,8]
    frontier = queue.Queue()
    frontier.put(start)
    came_from = dict()
    cost_so_far = dict()
    came_from[str(start[0])] = None
    cost_so_far[str(start[0])] = 0
    print(cost_so_far)

    current = [0,0]
    while not frontier.empty():
        current =frontier.get()[0]
        # print(current)
        # if current == goal:
        #     break
        for next in graph_neighbors(current,mapTest):
            if str(next[0]) not in came_from:
                frontier.put(next)
                came_from[str(next[0])] = current
                
            # + graph.cost(current,next)
            # print(next)
            # new_cost =cost_so_far[str(current)] + 1
            # if str(next[0]) not in cost_so_far or new_cost <cost_so_far[str(next[0])]:
            #     cost_so_far[str(next[0])] =new_cost
            #     priority = new_cost
            #     frontier.put(next, priority)
            #     came_from[str(next[0])] =current
        # print(came_from)
        # print(cost_so_far)
        print(">>>>>>>>>>>>>>>>")
    print(came_from)
    path = []
    current = goal
    while current != start[0]:
        path.append(current)
        print(current)
        current = came_from[str(current)]
    path.append(start[0])
    path.reverse()
    print(path)

    for i in path:
        print(i)
        p = Point(i[1],i[0])
        mapTest.setMap(p)
    mapTest.showMap();


